﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;
using System.IO;

namespace TSP
{
    public class AntColonyOptimization
    {
        private float _alpha;// = 3; // influence of pheromone on direction
        private float _beta;// = 2;  // influence of adjacent node distance
        private float _rho;// = 0.01f; // pheromone decrease factor
        private float _q;// = 2.0f;   // pheromone increase factor

        private int _numCities;
        private int _numAnts;
        private int _maxTime;
        private float _bestLength;       
        private float[][] dists;
        private int[][] ants;
        private int[] bestTrail;

        
        private float[][] pheromones;
        private int[] cities;

        public int NumCities
        {
            get { return _numCities; }
            set { _numCities = value; }
        }
        public int NumAnts
        {
            get { return _numAnts; }
            set { _numAnts = value; }
        }
        public int MaxTime
        {
            get { return _maxTime; }
            set { _maxTime = value; }
        }
        public int[] BestTrail
        {
            get { return bestTrail; }
            set { bestTrail = value; }
        }
        public float BestLength
        {
            get { return _bestLength; }
            set { _bestLength = value; }
        }

        public delegate void WriteResulToFile(string fileName, int[] chr, float fitness, Stopwatch time);
        public WriteResulToFile writeResult;

        public AntColonyOptimization(int numAnt, int numCities, int maxTime, float alpha, float beta, float rho, float q)
        {
            _numAnts = numAnt;
            _numCities = numCities;
            _maxTime = maxTime;
            _alpha = alpha;
            _beta = beta;
            _rho = rho;
            _q = q;
        }

        public int[] getCities(int star, int leng, int[] chr)
        {
            int[] cities = new int[leng];
            for (int ii = 0; ii < leng; ii++)
            {
                cities[ii] = chr[star + ii];
            }
            return cities;
        }

        public float[][] createMatrixDists(int num, int[] city)
        {
            float[][] dis = new float[num][];
            for (int i = 0; i < num; i++)
            {
                dis[i] = new float[num];
            }
            for (int i = 0; i < num; i++)
            {
                dis[i][i] = 0;
                for (int j = i + 1; j < num; j++)
                {
                    float d = InOutData.calculateDistance(city[i] - 1, city[j] - 1);
                    dis[i][j] = d;
                    dis[j][i] = d;
                }
            }
            return dis;
        }

        public void init(int num)
        {
            cities = new int[num];
            bestTrail = new int[num];
            dists = new float[num][];
            for (int ii = 0; ii < num; ii++)
            {
                dists[ii] = new float[num];
            }

        }

        private int indexOfTarget(int[] trail, int target) // helper for RandomTrail
        {
            for (int i = 0; i < trail.Length; ++i)
            {
                if (trail[i] == target)
                    return i;
            }
            throw new Exception("Target not found in IndexOfTarget");
        }

        public int[] randomTrail(int start, int numCities, Random ran) // helper for InitAnts
        {
            int[] trail = new int[numCities];
            for (int i = 0; i < numCities; ++i)
            {
                trail[i] = i;
            } // sequential
            for (int i = 0; i < numCities; ++i) // Fisher-Yates shuffle
            {
                int r = ran.Next(i, numCities);
                int tmp = trail[r]; trail[r] = trail[i]; trail[i] = tmp;
            }
            int idx = indexOfTarget(trail, start); // put start at [0]
            int temp = trail[0];
            trail[0] = trail[idx];
            trail[idx] = temp;
            return trail;
        }

        public int[][] initAnts(int numAnts, int numCities, Random ran)
        {
            int[][] ants = new int[numAnts][];
            for (int k = 0; k < numAnts; ++k)
            {
                int start = ran.Next(0, numCities);
                ants[k] = randomTrail(start, numCities, ran);
            }
            return ants;
        }

        private float distance(int cityX, int cityY, float[][] dists)
        {
            return dists[cityX][cityY];
        }

        private float calLengthTour(int leng, int[] trail, float[][] dists) // total length of tour
        {
            float result = 0.0f;
            for (int i = 0; i < leng - 1; i++)
                result += distance(trail[i], trail[i + 1], dists);
            result += distance(trail[0], trail[leng - 1], dists);
            return result;
        }

        private float calLengthTourGA(int start, int leng, int[] trail, float[][] dists, int[] chr)
        {
            float result = 0.0f;
            for (int i = 0; i < leng - 1; i++)
                result += distance(trail[i], trail[i + 1], dists);
            result += InOutData.calculateDistance(chr[start - 1] - 1, cities[trail[0]] - 1);
            result += InOutData.calculateDistance(cities[trail[leng - 1]] - 1, chr[start + leng] - 1);
            return result;
        }

        private int[] findBestTrail(int[][] ants, float[][] dists) // best trail has shortest total length, find in ants
        {
            float bestLength = calLengthTour(_numCities, ants[0], dists);
            int idxBestLength = 0;
            for (int k = 1; k < ants.Length; ++k)
            {
                float len = calLengthTour(_numCities, ants[k], dists);
                if (len < bestLength)
                {
                    bestLength = len;
                    idxBestLength = k;
                }
            }
            //int numCities = ants[0].Length;
            int[] bestTrail = new int[_numCities];
            ants[idxBestLength].CopyTo(bestTrail, 0);
            return bestTrail;
        }

        private int[] findBestTrailGA(int star, int length, int[][] ants, float[][] dists, int[] city) // best trail has shortest total length, find in ants
        {
            float bestLength = calLengthTourGA(star, length, ants[0], dists, city);
            int idxBestLength = 0;
            for (int k = 1; k < ants.Length; ++k)
            {
                float len = calLengthTourGA(star, length, ants[k], dists, city);
                if (len < bestLength)
                {
                    bestLength = len;
                    idxBestLength = k;
                }
            }
            //int numCities = ants[0].Length;
            int[] bestTrail = new int[_numCities];
            ants[idxBestLength].CopyTo(bestTrail, 0);
            return bestTrail;
        }

        private float[][] initPheromones(int numCities)
        {
            float[][] pheromones = new float[numCities][];
            for (int i = 0; i < numCities; ++i)
                pheromones[i] = new float[numCities];
            for (int i = 0; i < numCities; ++i)
                for (int j = 0; j < numCities; ++j)
                    pheromones[i][j] = 0.01f; // otherwise first call to UpdateAnts -> BuiuldTrail -> NextNode -> MoveProbs => all 0.0 => throws
            return pheromones;
        }

        private float[] moveProbs(int k, int cityX, bool[] visited, float[][] pheromones, float[][] dists)
        {
            // for ant k, located at nodeX, with visited[], return the prob of moving to each city
            // int numCities = pheromones.Length;
            float[] taueta = new float[_numCities]; // inclues cityX and visited cities
            float sum = 0.0f; // sum of all tauetas
            for (int i = 0; i < taueta.Length; i++) // i is the adjacent city
            {
                if (i == cityX)
                    taueta[i] = 0.0f; // prob of moving to self is 0
                else if (visited[i] == true)
                    taueta[i] = 0.0f; // prob of moving to a visited city is 0
                else
                {
                    taueta[i] = (float)Math.Pow(pheromones[cityX][i], _alpha) * (float)Math.Pow((1.0 / distance(cityX, i, dists)), _beta); // could be huge when pheromone[][] is big
                    if (taueta[i] < 0.0001)
                        taueta[i] = 0.0001f;
                    else if (taueta[i] > (float.MaxValue / (_numCities * 100)))
                        taueta[i] = float.MaxValue / (_numCities * 100);
                }
                sum += taueta[i];
            }

            float[] probs = new float[_numCities];
            for (int i = 0; i < probs.Length; ++i)
                probs[i] = taueta[i] / sum; // big trouble if sum = 0.0
            return probs;
        }

        private int nextCity(int k, int cityX, bool[] visited, float[][] pheromones, float[][] dists, Random ran)
        {
            // for ant k (with visited[]), at nodeX, what is next node in trail?
            float[] probs = moveProbs(k, cityX, visited, pheromones, dists);

            float[] cumul = new float[probs.Length + 1];
            for (int i = 0; i < probs.Length; i++)
                cumul[i + 1] = cumul[i] + probs[i]; // consider setting cumul[cuml.Length-1] to 1.00

            float p = (float)ran.NextDouble();
            if (p == 1.0f)
            {
                p = 0.99f;
            }
            for (int i = 0; i < cumul.Length - 1; i++)
                if (p >= cumul[i] && p < cumul[i + 1])
                    return i;
            throw new Exception("Failure to return valid city in NextCity");
        }

        private int[] buildTrail(int k, int start, float[][] pheromones, float[][] dists, Random ran)
        {
            // int numCities = pheromones.Length;
            int[] trail = new int[_numCities];
            bool[] visited = new bool[_numCities];
            trail[0] = start;
            visited[start] = true;
            for (int i = 0; i < _numCities - 1; ++i)
            {
                int cityX = trail[i];
                int next = nextCity(k, cityX, visited, pheromones, dists, ran);
                trail[i + 1] = next;
                visited[next] = true;
            }
            return trail;
        }

        private void updateAnts(int[][] ants, float[][] pheromones, float[][] dists, Random ran)
        {
            //int numCities = pheromones.Length;
            for (int k = 0; k < ants.Length; ++k)
            {
                int start = ran.Next(0, _numCities);
                int[] newTrail = buildTrail(k, start, pheromones, dists, ran);
                ants[k] = newTrail;
            }
        }            

        //kiem tra cityx va cityy co la 2 thanh pho ke nhau cua hanh trinh trail ko?
        private bool edgeInTrail(int cityX, int cityY, int[] trail)
        {
            // are cityX and cityY adjacent to each other in trail[]?
            int lastIndex = trail.Length - 1;
            int idx = indexOfTarget(trail, cityX);

            if (idx == 0 && trail[1] == cityY) return true;
            else if (idx == 0 && trail[lastIndex] == cityY) return true;
            else if (idx == 0) return false;
            else if (idx == lastIndex && trail[lastIndex - 1] == cityY) return true;
            else if (idx == lastIndex && trail[0] == cityY) return true;
            else if (idx == lastIndex) return false;
            else if (trail[idx - 1] == cityY) return true;
            else if (trail[idx + 1] == cityY) return true;
            else return false;
        }

        private void updatePheromones(float[][] pheromones, int[][] ants, float[][] dists)
        {
          for (int i = 0; i < pheromones.Length; ++i)
          {
            for (int j = i + 1; j < pheromones[i].Length; ++j)
            {
              for (int k = 0; k < ants.Length; ++k)
              {
                float length = calLengthTour(_numCities,ants[k], dists); // length of ant k trail
                float decrease = (1.0f - _rho) * pheromones[i][j];
                float increase = 0.0f;
                if (edgeInTrail(i, j, ants[k]) == true) increase = (_q / length);

                pheromones[i][j] = decrease + increase;

                if (pheromones[i][j] < 0.0001)
                  pheromones[i][j] = 0.0001f;
                else if (pheromones[i][j] > 100000.0)
                  pheromones[i][j] = 100000.0f;

                pheromones[j][i] = pheromones[i][j];
              }//for(k)
            }//for(j)
          }//for(i)
        }

        private void updatePheromonesGA(int start, int leng, float[][] pheromones, int[][] ants, float[][] dists, int[] city)
        {
            for (int i = 0; i < pheromones.Length; ++i)
            {
                for (int j = i + 1; j < pheromones[i].Length; ++j)
                {
                    for (int k = 0; k < ants.Length; ++k)
                    {
                        float length = calLengthTourGA(start, leng,ants[k], dists, city); // length of ant k trail
                        float decrease = (1.0f - _rho) * pheromones[i][j];
                        float increase = 0.0f;
                        if (edgeInTrail(i, j, ants[k]) == true) increase = (_q / length);

                        pheromones[i][j] = decrease + increase;

                        if (pheromones[i][j] < 0.0001)
                            pheromones[i][j] = 0.0001f;
                        else if (pheromones[i][j] > 100000.0)
                            pheromones[i][j] = 100000.0f;

                        pheromones[j][i] = pheromones[i][j];
                    }//for(k)
                }//for(j)
            }//for(i)
        }

        private void displayTrail(int[] trail)
        {
          for (int i = 0; i < trail.Length; ++i)
          {
            Console.Write(trail[i] + " ");
            if (i > 0 && i % 20 == 0) Console.WriteLine("");
          }
          Console.WriteLine("");
        }

        public void antColony(Random ran)
        {
            init(_numCities);
            for (int ii = 0; ii < _numCities; ii++)
            {
                cities[ii] = ii + 1;
            }
            // Initialing dummy graph distances
            dists = createMatrixDists(_numCities, cities);

            // Initialing ants to random trails\n"
            ants = initAnts(_numAnts, _numCities, ran); // initialize ants to random trails
            
            bestTrail = findBestTrail(ants, dists);   // determine the best initial trail

            _bestLength = calLengthTour(_numCities, bestTrail, dists); // the length of the best trail           

            // Initializing pheromones on trails
            pheromones  = initPheromones(_numCities);

            int time = 0;            
            while (time < _maxTime)
            {
              updateAnts(ants, pheromones, dists, ran);
              updatePheromones(pheromones, ants, dists);

              int[] currBestTrail = findBestTrail(ants, dists);
              float currBestLength = calLengthTour(_numCities, currBestTrail, dists);
              if (currBestLength < _bestLength)
              {
                _bestLength = currBestLength;
                bestTrail = currBestTrail;
                //Console.WriteLine("New best length of " + _bestLength.ToString("F1") + " found at time " + time);
              }
              ++time;
            }
            
            //displayTrail(bestTrail);
            //Console.WriteLine("\nLength of best trail found: " + _bestLength.ToString("F1"));

            //Console.WriteLine("\nEnd Ant Colony Optimization demo\n");
            //Console.ReadLine();
       }

        public void antColonyGA(int star, int leng, int[] chr, Random ran)
        {
            init(_numCities);
            cities = getCities(star, leng, chr);
            // Initialing dummy graph distances
            dists = createMatrixDists(_numCities, cities);

            // Initialing ants to random trails\n"
            ants = initAnts(_numAnts, _numCities, ran); // initialize ants to random trails

            bestTrail = findBestTrailGA(star, leng, ants, dists, cities);   // determine the best initial trail

            _bestLength = calLengthTourGA(star, leng, bestTrail, dists, chr); // the length of the best trail           

            // Initializing pheromones on trails
            pheromones = initPheromones(_numCities);

            int time = 0;
            while (time < _maxTime)
            {
                updateAnts(ants, pheromones, dists, ran);
                updatePheromones(pheromones, ants, dists);

                int[] currBestTrail = findBestTrailGA(star, leng, ants, dists, cities);
                float currBestLength = calLengthTourGA(star, leng, currBestTrail, dists, chr);
                if (currBestLength < _bestLength)
                {
                    _bestLength = currBestLength;
                    bestTrail = currBestTrail;
                    //Console.WriteLine("New best length of " + _bestLength.ToString("F1") + " found at time " + time);
                }
                ++time;
            }
            
        }
        



    }
}
